/*------------------------------------------------------------------------------------------*\
   This file contains material supporting chapter 8 of the cookbook:
   Computer Vision Programming using the OpenCV Library.
   by Robert Laganiere, Packt Publishing, 2011.

   This program is free software; permission is hereby granted to use, copy, modify,
   and distribute this source code, or portions thereof, for any purpose, without fee,
   subject to the restriction that the copyright notice may not be removed
   or altered from any source or altered source distribution.
   The software is released on an as-is basis and without any warranties of any kind.
   In particular, the software is not guaranteed to be fault-tolerant or free from failure.
   The author disclaims all warranties with regard to this software, any use,
   and any consequent failure, is purely the responsibility of the user.
 

   Copyright (C) 2010-2011 Robert Laganiere, www.laganiere.name
\*------------------------------------------------------------------------------------------*/

#if !defined HARRISD
#define HARRISD

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>

class HarrisDetector
{

  private:
    // 32-bit float image of corner strength
    cv::Mat cornerStrength;
    // 32-bit float image of thresholded corners
    cv::Mat cornerTh;
    // image of local maxima (internal)
    cv::Mat localMax;
    // size of neighbourhood for derivatives smoothing
    int neighbourhood;
    // aperture for gradient computation
    int aperture;
    // Harris parameter
    double k;
    // maximum strength for threshold computation
    double maxStrength;
    // calculated threshold (internal)
    double threshold;
    // size of neighbourhood for non-max suppression
    int nonMaxSize;
    // kernel for non-max suppression
    cv::Mat kernel;

  public:
    HarrisDetector()
        : neighbourhood(3), aperture(3), k(0.1), maxStrength(0.0), threshold(0.01), nonMaxSize(3)
    {

        setLocalMaxWindowSize(nonMaxSize);
    }

    // Create kernel used in non-maxima suppression
    void setLocalMaxWindowSize(int size)
    {

        nonMaxSize = size;
        kernel.create(nonMaxSize, nonMaxSize, CV_8U);
    }

    // Compute Harris corners
    void detect(const cv::Mat& image)
    {

        // Harris computation
        cv::cornerHarris(image, cornerStrength,
                         neighbourhood, // neighborhood size
                         aperture,      // aperture size
                         k);            // Harris parameter

        // internal threshold computation
        double minStrength; // not used
        cv::minMaxLoc(cornerStrength, &minStrength, &maxStrength);

        // local maxima detection
        cv::Mat dilated; // temporary image
        cv::dilate(cornerStrength, dilated, cv::Mat());
        cv::compare(cornerStrength, dilated, localMax, cv::CMP_EQ);
    }

    // Get the corner map from the computed Harris values
    cv::Mat getCornerMap(double qualityLevel)
    {

        cv::Mat cornerMap;

        // thresholding the corner strength
        threshold = qualityLevel * maxStrength;
        cv::threshold(cornerStrength, cornerTh, threshold, 255, cv::THRESH_BINARY);

        // convert to 8-bit image
        cornerTh.convertTo(cornerMap, CV_8U);

        // non-maxima suppression
        cv::bitwise_and(cornerMap, localMax, cornerMap);

        return cornerMap;
    }

    // Get the feature points vector from the computed Harris values
    void getCorners(std::vector<cv::Point>& points, double qualityLevel)
    {

        // Get the corner map
        cv::Mat cornerMap = getCornerMap(qualityLevel);
        // Get the corners
        getCorners(points, cornerMap);
    }

    // Get the feature points vector from the computed corner map
    void getCorners(std::vector<cv::Point>& points, const cv::Mat& cornerMap)
    {

        // Iterate over the pixels to obtain all feature points
        for (int y = 0; y < cornerMap.rows; y++)
        {

            const uchar* rowPtr = cornerMap.ptr<uchar>(y);

            for (int x = 0; x < cornerMap.cols; x++)
            {

                // if it is a feature point
                if (rowPtr[x])
                {

                    points.push_back(cv::Point(x, y));
                }
            }
        }
    }

    // Draw circles at feature point locations on an image
    void drawOnImage(cv::Mat& image, const std::vector<cv::Point>& points,
                     cv::Scalar color = cv::Scalar(255, 255, 255), int radius = 3,
                     int thickness = 2)
    {

        std::vector<cv::Point>::const_iterator it = points.begin();

        // for all corners
        while (it != points.end())
        {

            // draw a circle at each corner location
            cv::circle(image, *it, radius, color, thickness);
            ++it;
        }
    }
};

#endif
